/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#include "KF.h"
namespace hitcrt {
/**
 * @brief 状态更新--添加异常处理
 * @param {null}
 * @return
 * @author www
 */
void KF::stateUpdate() {
    try {
        checkStateUpdate();
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        throw ex; // 重新抛出异常以便上层捕获
        return;
    }
    stateUpdateImpl();
}
/**
 * @brief 测量更新--添加异常处理
 * @param {null}
 * @return
 * @author www
 */
void KF::measureUpdate() {
    try {
        checkMeasureUpdate();
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        return;
    }
    measureUpdateImpl();
}
/**
 * @brief 状态更新方程异常处理
 * @param {null}
 * @return stateUpdateDimErr 状态更新方程异常类，输出对应维度异常的等式
 * @author www
 */
void KF::checkStateUpdate() {
    if (m_transitionMatrix.cols() != m_statePost.rows()) {
        throw stateUpdateDimErr("first");
    }
    if (m_transitionMatrix.cols() != m_errorCovPost.rows() ||
        m_errorCovPost.cols() != m_transitionMatrix.cols() ||
        m_transitionMatrix.rows() != m_processNoiseCov.rows() ||
        m_transitionMatrix.cols() != m_processNoiseCov.cols()) {
        throw stateUpdateDimErr("second");
    }
    if (m_transitionMatrix.cols() != m_errorCovPrior.rows()) {
        throw stateUpdateDimErr("third");
    }
}
/**
 * @brief 测量更新方程异常处理
 * @param {null}
 * @return measUpdateDimErr 测量更新方程异常类，输出对应维度异常的等式
 * @author www
 */
void KF::checkMeasureUpdate() {
    if (m_measurementMatrix.cols() != m_errorCovPrior.rows() ||
        m_measurementMatrix.cols() != m_errorCovPrior.cols() ||
        m_measurementMatrix.rows() != m_measurementNoiseCov.rows() ||
        m_measurementMatrix.rows() != m_measurementNoiseCov.cols())
        throw measUpdateDimErr("first");
    if (m_measurement.rows() != m_measurementMatrix.rows() || m_measurement.cols() != m_statePrior.cols() ||
        m_measurementMatrix.cols() != m_statePrior.rows() || m_statePrior.cols() != m_measurement.cols() ||
        m_statePrior.rows() != m_gain.rows() || m_gain.cols() != m_measurement.rows())
        throw measUpdateDimErr("second");
    if (m_gain.cols() != m_measurementMatrix.rows() || m_errorCovPrior.rows() != m_measurementMatrix.cols() ||
        m_errorCovPrior.cols() != m_measurementMatrix.cols() ||
        m_gain.cols() != m_measurementNoiseCov.rows() || m_measurementNoiseCov.cols() != m_gain.cols())
        throw measUpdateDimErr("third");
}
/**
 * @brief 时间更新方程，对应预测阶段
 * @param {null}
 * @return {null}
 * @author www
 */
void KF::stateUpdateImpl() {
    /*时间更新方程，卡尔曼滤波为一种高斯滤波，均值和协方差可以表示一个高斯分布*/
    /// X'(k+1|k)=F(k)X(k|k)+G(k)*a_post predict_value  状态更新得到先验证均值（状态）更新
    if (m_conExist) {
        m_statePrior = m_transitionMatrix * m_statePost + m_controlledMatrix * m_controlledVec;
    } else {
        m_statePrior = m_transitionMatrix * m_statePost;
    }
    /// 状态更新得到先验协方差估计
    m_errorCovPrior =
        m_transitionMatrix * m_errorCovPost * m_transitionMatrix.transpose() + m_processNoiseCov;
    // 用于观测方程残差计算，放在此处便于子类修改变量
    m_transformation = m_measurementMatrix * m_statePrior;

    /**
     * @brief 无需修改，代码完整
     * @author Rzihan (jiuren812@qq.com)
     */
    // throw std::runtime_error("Not implemented yet at "+std::string(__FILE__)+":"+std::to_string(__LINE__)); // TODO 将这行替换为实际初始化代码
    // 估计为 3 到 20 行
    // 要考虑 m_conExist
}
/**
 * @brief 测量更新方程，对应更新阶段
 * @param {null}
 * @return {null}
 * @author www
 */
void KF::measureUpdateImpl() {
        // 计算中间变量
    const Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_stateDimension, m_stateDimension);
    /***状态更新方程*/
    const Eigen::MatrixXd temp4 =
        m_measurementMatrix * m_errorCovPrior * m_measurementMatrix.transpose() + m_measurementNoiseCov;
    // 卡尔曼增益
    m_gain = m_errorCovPrior * m_measurementMatrix.transpose() * temp4.inverse();

    /// m_residual=z(k+1)-H(k)X'(k+1|k)  残差
    m_residual = m_measurement - m_transformation;
    /// 后验状态估计（高斯均值）
    m_statePost = m_statePrior + m_gain * m_residual;

    const Eigen::MatrixXd I_KH = I - m_gain * m_measurementMatrix;
    /// 后验协方差矩阵
    m_errorCovPost =
        I_KH * m_errorCovPrior * I_KH.transpose() + m_gain * m_measurementNoiseCov * m_gain.transpose();
    // m_errorCovPost = (I - m_gain * m_measurementMatrix) * m_errorCovPrior; 可能不正定，故使用上式而非该式

    /**
     * @brief 无需修改，代码完整
     * @author Rzihan (jiuren812@qq.com)
     */
    // throw std::runtime_error("Not implemented yet at "+std::string(__FILE__)+":"+std::to_string(__LINE__)); // TODO 将这行替换为实际初始化代码
    // 估计为 10 到 30 行
}
/**
 * @brief 完整的滤波过程
 * @param {null}
 * @return {null}
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void KF::predict() {
    stateUpdate();
    measureUpdate();
}
/**
 * @brief 用户访问滤波结果
 * 此处输出后验状态估计结果
 * @param {null}
 * @return {null}
 * @author www
 * -phone:17390600977;qq:1922039181
 */
Eigen::MatrixXd &KF::getResult() { return m_statePost; }

}  // namespace hitcrt
